/* Copyright (c) 2010 - 2020, Nordic Semiconductor ASA
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 *
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "light_lc_light_pi.h"

#include "log.h"
#include "light_lightness_utils.h"
#include "light_lc_state_utils.h"
#include "light_lc_server_property_constants.h"

#include "nrf_mesh_config_core.h"

/** Define to 1 to see debug information - helpful when setting up new hardware or tuning an installation */
#ifndef LIGHT_PI_DEBUG
#define LIGHT_PI_DEBUG 0
#endif

/**
 * Minimum value for the integral term of the Light LC Pi Feedback Regulator.
 *
 * This value is used for the non-agressive anti-reset windup for the integral term.
 *
 * @tagMeshMdlSp, Table 6.53 defines minimum value as 0.
 */
#define LIGHT_PI_INTEGRAL_MIN              (0)

/**
 * Maximum value for the integral term of the Light LC Pi Feedback Regulator.
 *
 * This value is used for the non-agressive anti-reset windup for the integral term.
 *
 * @tagMeshMdlSp, Table 6.53 defines maximum value as 65535.
 */
#define LIGHT_PI_INTEGRAL_MAX              (65535)

/** Fixed point multiplier for coefficients */
#define LIGHT_PI_COEFFICIENT_MULT (100)

/** @tagMeshMdlSp requires I be 0-65535 (Table 6.53). */
#define ANTI_RESET_WINDUP(i) MAX(MIN(LIGHT_PI_INTEGRAL_MAX, i), LIGHT_PI_INTEGRAL_MIN)

NRF_MESH_STATIC_ASSERT(LIGHT_LC_LIGHT_PI_SUMMATION_INTERVAL_MS >= LIGHT_LC_LIGHT_PI_SUMMATION_INTERVAL_MIN_MS);
NRF_MESH_STATIC_ASSERT(LIGHT_LC_LIGHT_PI_SUMMATION_INTERVAL_MS <= LIGHT_LC_LIGHT_PI_SUMMATION_INTERVAL_MAX_MS);

/* Return a fixed point value (if float value was 250.5, return integer of 25050) */
static uint32_t coefficient_get(light_lc_setup_server_t * p_s_server, uint16_t property_id)
{
    uint32_t coefficient_word;
    uint32_t coefficient_int;
    float * p_coefficient_float;
    float coefficient_float;

    coefficient_word =  light_lc_state_utils_property_get(p_s_server, property_id);
    p_coefficient_float = (float *) &coefficient_word;
    coefficient_float = *p_coefficient_float;
    coefficient_float *= LIGHT_PI_COEFFICIENT_MULT;
    coefficient_int = (uint32_t) coefficient_float;
    return coefficient_int;
}

/* Pass in the current linear lightness, and the one generated by light PI, and this function will
 * decide what to set the light level to */
static void linear_binding_output_set(light_lc_setup_server_t * p_s_server,
                                      uint16_t current_linear_lightness,
                                      uint16_t linear_lightness_val)
{
    uint16_t lightness_out;
    uint16_t actual_lightness;

    /* If the passed in linear lightness value is greated than the one computed by the state
     * machine, set the passed in one. Otherwise, use the one computed by the state machine.  This
     * fulfills the "MAX" box in figure 6.5 @tagMeshMdlSp */

    /* But first, remember that we only conditionally bind to the lightness linear state - so check
     * if we have disabled that binding */
    if (light_lc_state_utils_server_control_is_disabled(p_s_server))
    {
        /* control is disabled - don't set the lightness linear state */
        return;
    }

    /* The state machine did its computation in linear, so we don't
     * need to convert before comparing to linear_lightness_val*/
    lightness_out = light_lc_state_utils_lightness_out_get(p_s_server);

#if LIGHT_PI_DEBUG
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "FSM-out linear=0x%04X, pi_generated=0x%0X\n", lightness_out,
          linear_lightness_val);
#endif

    if (linear_lightness_val > lightness_out)
    {
        lightness_out = linear_lightness_val;
    }

    /* Only set the lightness value if it is different from the current lightness */
    if (current_linear_lightness != lightness_out)
    {
        /* Linear output is computed by the LC server, but the mid-app is expecting human perceptive
         * (actual) values */
        actual_lightness = light_lightness_utils_linear_to_actual(lightness_out);

#if LIGHT_PI_DEBUG
        __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "computed actual=0x%0X\n", lightness_out,
              linear_lightness_val);
#endif

        /* Ask the system to set the LED to the computed value */
        p_s_server->settings.p_callbacks->light_lc_cbs.light_lc_actual_set_cb(p_s_server, actual_lightness);
    }
}

void light_lc_light_pi_update(light_lc_setup_server_t * p_s_server)
{
    static int32_t i_internal_sum = 0;

    /* Valid values are 0.0-100.0, all other values invalid - Percentage 8 */
    uint8_t regulator_accuracy;
    uint32_t luxlevel_out;
    uint32_t ambient_luxlevel_illum;
    uint16_t actual_lightness;
    int32_t e_adjustment_error;
    uint32_t d_accuracy;
    int32_t u_regulator_input;
    int32_t l_regulator_output;
    uint32_t t_summation_interval;
    uint32_t k_iu;
    uint32_t k_id;
    uint32_t k_pu;
    uint32_t k_pd;
    uint16_t current_lightness_linear;
    uint16_t computed_lightness_linear;
    int32_t computed_lightness_linear_diff;

    if (p_s_server == NULL)
    {
        return;
    }

    /* Get the current lightness (this is what has contributed into our lightness value we received
     * from the sensor) */
    p_s_server->settings.p_callbacks->light_lc_cbs.light_lc_actual_get_cb(p_s_server, &actual_lightness);
    current_lightness_linear = light_lightness_utils_actual_to_linear(actual_lightness);

#if LIGHT_PI_DEBUG
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "got current actual 0x%04X, linear 0x%04X\n",
          actual_lightness, current_lightness_linear);
#endif

    if (!light_lc_state_utils_ambient_luxlevel_is_valid(p_s_server))
    {
        /* couldn't find any valid ambient light data for product-specific definition */
#if LIGHT_PI_DEBUG
        __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "Can't find any measurement - don't set any PI value \n");
#endif
        /* Set the FSM-generated lightness values, and return */
        linear_binding_output_set(p_s_server, current_lightness_linear, 0);
        return;
    }

    ambient_luxlevel_illum = light_lc_state_utils_ambient_luxlevel_get(p_s_server);

#if LIGHT_PI_DEBUG
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "ambient luxlevel %d\n", ambient_luxlevel_illum);
#endif

    /* get the values out of the states for the luxlevel and lightness */
    luxlevel_out = light_lc_state_utils_luxlevel_out_get(p_s_server);

    e_adjustment_error = (int64_t) luxlevel_out - (int64_t) ROUNDED_DIV(ambient_luxlevel_illum, LIGHT_PI_COEFFICIENT_MULT);

    regulator_accuracy = light_lc_state_utils_property_get(p_s_server, LIGHT_LC_SERVER_REGULATOR_ACCURACY_PID);

    /* Divide by 2 since regulator_accuracy is is percentage 8, then /2 since width is 2D wide (-D
     * <=> +D) Then / 100 to use as a percentage */
    d_accuracy = (regulator_accuracy * luxlevel_out / 2) / 2 / 100;

    if (e_adjustment_error > (int32_t) d_accuracy)
    {
        u_regulator_input = e_adjustment_error - d_accuracy;
    }
    else if (e_adjustment_error < (int32_t) (0 - d_accuracy))
    {
        u_regulator_input = e_adjustment_error + d_accuracy;
    }
    else
    {
        u_regulator_input = 0;
    }
#if LIGHT_PI_DEBUG
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "Accuracy=%d, D=%d, Luxlevel SP=%d, measured=%d, E=%d, U=%d\n",
          regulator_accuracy, d_accuracy, ROUNDED_DIV(luxlevel_out, LIGHT_PI_COEFFICIENT_MULT),
          ROUNDED_DIV(ambient_luxlevel_illum, LIGHT_PI_COEFFICIENT_MULT), e_adjustment_error, u_regulator_input);
#endif

    t_summation_interval = LIGHT_LC_LIGHT_PI_SUMMATION_INTERVAL_MS;
    /* Convert the Coefficient properties (floats) to fixed point integers (1/100s) */
    k_iu = coefficient_get(p_s_server, LIGHT_LC_SERVER_REGULATOR_KIU_PID);
    k_id = coefficient_get(p_s_server, LIGHT_LC_SERVER_REGULATOR_KID_PID);
    k_pu = coefficient_get(p_s_server, LIGHT_LC_SERVER_REGULATOR_KPU_PID);
    k_pd = coefficient_get(p_s_server, LIGHT_LC_SERVER_REGULATOR_KPD_PID);

    if (u_regulator_input >= 0)
    {
        /* Divide by 1000 to convert msec to sec, divide by coefficient fixed point */
        i_internal_sum +=
            ((int64_t) u_regulator_input * (int64_t) t_summation_interval * (int64_t) k_iu / 1000) / LIGHT_PI_COEFFICIENT_MULT;
        l_regulator_output =
            i_internal_sum + ((int64_t) u_regulator_input * (int64_t) k_pu) / LIGHT_PI_COEFFICIENT_MULT;
#if LIGHT_PI_DEBUG
        __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "U >= 0: I=%d, L=%d Kiu(*%d)=%d Kpu(*%d)=%d\n",
              i_internal_sum, l_regulator_output, LIGHT_PI_COEFFICIENT_MULT, k_iu,
              LIGHT_PI_COEFFICIENT_MULT, k_pu);
#endif
    }
    else
    {
        /* Divide by 1000 to convert msec to sec, divide by LIGHT_PI_COEFFICIENT_MULT for coefficient fixed point */
        i_internal_sum +=
            ((int64_t) u_regulator_input * (int64_t) t_summation_interval * (int64_t) k_id / 1000) / LIGHT_PI_COEFFICIENT_MULT;
        l_regulator_output = i_internal_sum + ((int64_t) u_regulator_input * (int64_t) k_pd) / LIGHT_PI_COEFFICIENT_MULT;
#if LIGHT_PI_DEBUG
        __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "U < 0: I=%d, L=%d Kid(*%d)=%d Kpd(*%d)=%d\n",
              i_internal_sum, l_regulator_output, LIGHT_PI_COEFFICIENT_MULT, k_id,
              LIGHT_PI_COEFFICIENT_MULT, k_pd);
#endif
    }

    i_internal_sum = ANTI_RESET_WINDUP(i_internal_sum);
#if LIGHT_PI_DEBUG
    __LOG(LOG_SRC_APP, LOG_LEVEL_INFO, "after anti reset windup, I=%d\n", curr_i_internal_sum);
#endif

    /* Conversion function from Lux to linear lightness depends on the Ambient light sensor distance
     * and angle from the light - use customer-tuned divisor */
    computed_lightness_linear_diff = l_regulator_output / LIGHT_LC_LIGHT_PI_CONVERSION_DIVISOR;

    computed_lightness_linear = current_lightness_linear + computed_lightness_linear_diff;

    if (computed_lightness_linear_diff < 0)
    {
        /* Negative value - snap any underflow to 0 (can't go negative) */
        if (abs(computed_lightness_linear_diff) > current_lightness_linear)
        {
            computed_lightness_linear = 0;
        }
    }
    else
    {
        /* Positive value - clip any overflow of the uint16 */
        if ((UINT16_MAX - current_lightness_linear) < computed_lightness_linear_diff)
        {
            computed_lightness_linear = UINT16_MAX;
        }
    }
    linear_binding_output_set(p_s_server, current_lightness_linear, computed_lightness_linear);
}
